#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

Point3f uv2xyz(Point2f uvl, Point2f uvr); //或求解pnp得到坐标转换矩阵

float leftIntrinsic[3][3] = {4037.82450,			 0,		947.65449,
									  0,	3969.79038,		455.48718,
									  0,			 0,				1};
//左相机畸变系数
float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};
//左相机旋转矩阵
float leftRotation[3][3] = {0.912333,		-0.211508,		 0.350590, 
							0.023249,		-0.828105,		-0.560091, 
							0.408789,		 0.519140,		-0.750590};
//左相机平移向量
float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};
 
//右相机内参数矩阵
float rightIntrinsic[3][3] = {3765.83307,			 0,		339.31958,
										0,	3808.08469,		660.05543,
										0,			 0,				1};
//右相机畸变系数
float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};
//右相机旋转矩阵
float rightRotation[3][3] = {-0.134947,		 0.989568,		-0.050442, 
							  0.752355,		 0.069205,		-0.655113, 
							 -0.644788,		-0.126356,		-0.753845};
//右相机平移向量
float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};


Point3f uv2xyz(cv::Point2f uvl, cv::Point2f uvr)
{
    Mat mLeftRotation=Mat(3,3,CV_64F,leftRotation);
    Mat mLeftTranslation = Mat(3,1,CV_64F, leftTranslation);
    Mat mLeftRT=Mat(3,4,CV_64F);
    hconcat(mLeftRotation,mLeftTranslation,mLeftRT);
    Mat mLeftIntrinsic=Mat(3,3,CV_64F,leftIntrinsic);
    Mat mLeftM=mLeftIntrinsic*mLeftRT;
    
    Mat mRightRotation=Mat(3,3,CV_64F,rightRotation);
    Mat mRightTranslation=Mat(3,1,CV_64F,rightTranslation);
    Mat mRightRT=Mat(3,4,CV_64F,);
    hconcat(mRightRotation,mRightTranslation,mRightRT);
    Mat mRightIntrinsic=Mat(3,3,CV_64F,rightIntrinsic);
    Mat mRightM=mRightIntrinsic*mRightRT;
    
    Mat A =Mat(4,3,CV_32F);
    A.at<float>(0,0)=uvl.x*mLeftM.at<float>(2,0)-mLeftM.at<float>(0,0);
    A.at<float>(0,1)=uvl.x*mLeftM.at<float>(2,1)-mLeftM.at<float>(0,1);
    A.at<float>(0,2)=uvl.x*mLeftM.at<float>(2,2)-mLeftM.at<float>(0,2);
    
    A.at<float>(1,0)=uvl.x*mLeftM.at<float>(2,0)-mLeftM.at<float>(1,0);
    A.at<float>(1,1)=uvl.x*mLeftM.at<float>(2,1)-mLeftM.at<float>(1,1);
    A.at<float>(1,2)=uvl.x*mLeftM.at<float>(2,2)-mLeftM.at<float>(1,2);
    
    A.at<float>(0,0)=uvl.x*mRightM.at<float>(2,0)-mRightM.at<float>(0,0);
    A.at<float>(0,1)=uvl.x*mRightM.at<float>(2,1)-mRightM.at<float>(0,1);
    A.at<float>(0,2)=uvl.x*mRightM.at<float>(2,2)-mRightM.at<float>(0,2);
    
    A.at<float>(1,0)=uvl.x*mRightM.at<float>(2,0)-mRightM.at<float>(1,0);
    A.at<float>(1,1)=uvl.x*mRightM.at<float>(2,1)-mRightM.at<float>(1,1);
    A.at<float>(1,2)=uvl.x*mRightM.at<float>(2,2)-mRightM.at<float>(1,2);
    
    Mat B=Mat(4,1,CV_64F);
    B.at<float>(0,0)=mLeftM.at<float>(0,3)-uvl.x*mLeftM.at<float>(2,3);
    B.at<float>(1,0)=mLeftM.at<float>(1,3)-uvl.y*mLeftM.at<float>(2,3);
    B.at<float>(2,0)=mRightM.at<float>(0,3)-uvr.x*mRightM.at<float>(2,3);
    B.at<float>(3,0)=mRightM.at<float>(1,3)-uvr.y*mRightM.at<float>(2,3);
    
    Mat XYZ=Mat(3,1,CV_64F);
    solve(A,B,XYZ,DECOMP_SVD);
    Point3f world;
    world.x=XYZ.at<float>(0,0);
    world.y=XYZ.at<float>(1,0);
    world.z=XYZ.at<float>(2,0);
    
    return world;
}


int main(int argc, char **argv) {
    std::cout << "Hello, world!" << std::endl;
    return 0;
}
